#include "stdafx.h"
#include "SteeringBehavior.h"
#include "Boid.h"
#include "World.h"
#include "Wall3DStrip.h"
//#include "ParamLoader.h"

//#include <cassert>

using std::string;
using std::vector;

//------------------------------------------------------------------------
//	helper functions
//
//------------------------------------------------------------------------
MyVec3 PointToLocalSpace(const MyVec3& point,
						MyVec3& agentHeading,
						MyVec3& agentSide,
						MyVec3& agentPosition)
{
	// keep a point copy
	MyVec3 transPoint(point);

	//create a transformation matrix
	MyCommon::Matrix4 mat(MyCommon::Matrix4::ZERO);

	MyReal tx = -agentPosition.dotProduct(agentHeading);
	MyReal tz = -agentPosition.dotProduct(agentSide);

	//create the transformation matrix
	mat[0][0] = agentHeading[0];	mat[0][2] = agentSide[0];
	mat[2][0] = agentHeading[2];	mat[2][2] = agentSide[2];
	mat[3][0] = tx;					mat[3][2] = tz;

	transPoint = point*mat;
	return transPoint;
}

MyVec3 VectorToWorldSpace(const MyVec3 &vec,
						  const MyVec3 &AgentHeading,
						  const MyVec3 &AgentSide)
{
	//make a copy of the point
	MyVec3 TransVec = vec;

	//create a transformation matrix
	MyCommon::Matrix4 mat(MyCommon::Matrix4::IDENTITY);

	//rotate
	//mat.Rotate(AgentHeading, AgentSide);
	mat[0][0] = AgentHeading[0];	mat[0][2] = AgentHeading[2];
	mat[2][0] = AgentSide[0];		mat[2][2] = AgentSide[2];

	//now transform the vertices
	//mat.TransformVector2(TransVec);
	TransVec = mat*vec;

	return TransVec;
}


//------------------------------------------------------------------------
//	constructor
//
//------------------------------------------------------------------------
SteeringBehavior::SteeringBehavior(Boid* agent)
:mBoid(agent),
miFlags(0),
mdDBoxLength(Prm.MinDetectionBoxLength),
mdWeightCohesion(Prm.CohesionWeight),
mdWeightAlignment(Prm.AlignmentWeight),
mdWeightSeparation(Prm.SeparationWeight),
mdWeightObstacleAvoidance(Prm.ObstacleAvoidanceWeight),
mdWeightWander(Prm.WanderWeight),
mdWeightWallAvoidance(Prm.WallAvoidanceWeight),
mdViewDistance(Prm.ViewDistance),
mdWallDetectionFeelerLength(Prm.WallDetectionFeelerLength),
mFeelers(3),
mDeceleration(normal),
//mpTargetAgent1(NULL),
//mpTargetAgent2(NULL),
mdWanderDistance(WanderDist),
mdWanderJitter(WanderJitterPerSec),
mdWanderRadius(WanderRad),
mdWaypointSeekDistSq(WaypointSeekDist*WaypointSeekDist),
mdWeightSeek(Prm.SeekWeight),
mdWeightFlee(Prm.FleeWeight),
mdWeightArrive(Prm.ArriveWeight),
mdWeightPursuit(Prm.PursuitWeight),
mdWeightOffsetPursuit(Prm.OffsetPursuitWeight),
mdWeightInterpose(Prm.InterposeWeight),
mdWeightHide(Prm.HideWeight),
mdWeightEvade(Prm.EvadeWeight),
mdWeightFollowPath(Prm.FollowPathWeight),
//mbCellSpaceOn(false),
mSummingMethod(prioritized)
{
	//stuff for the wander behavior
	MyReal theta = MyCommon::Math::UnitRandom() * MyCommon::Math::TWO_PI;

	//create a vector to a target position on the wander circle
	mvWanderTarget = MyVec3(
		mdWanderRadius * MyCommon::Math::Cos(theta), 
		0,
		mdWanderRadius * MyCommon::Math::Sin(theta)
		);

	//create a Path
	mpPath = new Path();
	mpPath->LoopOn();
}

//------------------------------------------------------------------------
//	destructor
//
//------------------------------------------------------------------------
SteeringBehavior::~SteeringBehavior()
{
	delete mpPath;
}


//----------------------- Calculate --------------------------------------
//
//  calculates the accumulated steering force according to the method set
//  in mSummingMethod
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Calculate()
{
	//reset the steering force
	mSteeringForce = MyVec3::ZERO;

	////use space partitioning to calculate the neighbors of this Boid
	////if switched on. If not, use the standard tagging system
	//if (!isSpacePartitioningOn())
	//{
	//	//tag neighbors if any of the following 3 group behaviors are switched on
	//	if (On(separation) || On(allignment) || On(cohesion))
	//	{
	//		mBoid->getWorld()->TagVehiclesWithinViewRange(mBoid, mdViewDistance);
	//	}
	//}
	//else
	//{
	//	//calculate neighbors in cell-space if any of the following 3 group
	//	//behaviors are switched on
	//	if (On(separation) || On(allignment) || On(cohesion))
	//	{
	//		mBoid->getWorld()->CellSpace()->CalculateNeighbors(mBoid->getPos(), mdViewDistance);
	//	}
	//}

	switch (mSummingMethod)
	{
	case weighted_average:

		mSteeringForce = CalculateWeightedSum(); break;

	case prioritized:

		mSteeringForce = CalculatePrioritized(); break;

	case dithered:

		mSteeringForce = CalculateDithered();break;

	default:mSteeringForce = MyVec3::ZERO;
	}

	return mSteeringForce;
}

//------------------------- ForwardComponent -----------------------------
//
//  returns the forward component of the steering force
//------------------------------------------------------------------------
MyReal SteeringBehavior::ForwardComponent()
{
	return mBoid->getHeading().dotProduct(mSteeringForce);
}

//--------------------------- SideComponent ------------------------------
//  returns the side component of the steering force
//------------------------------------------------------------------------
MyReal SteeringBehavior::SideComponent()
{
	return mBoid->getSide().dotProduct(mSteeringForce);
}

//--------------------- AccumulateForce ----------------------------------
//
//  This function calculates how much of its max steering force the 
//  Boid has left to apply and then applies that amount of the
//  force to add.
//------------------------------------------------------------------------
bool SteeringBehavior::AccumulateForce(MyVec3 &RunningTot, MyVec3 ForceToAdd)
{
	//calculate how much steering force the Boid has used so far
	MyReal MagnitudeSoFar = RunningTot.length();

	//calculate how much steering force remains to be used by this Boid
	MyReal MagnitudeRemaining = mBoid->getMaxForce() - MagnitudeSoFar;

	//return false if there is no more force left to use
	if (MagnitudeRemaining <= 0.0) return false;

	//calculate the magnitude of the force we want to add
	MyReal MagnitudeToAdd = ForceToAdd.length();

	//if the magnitude of the sum of ForceToAdd and the running total
	//does not exceed the maximum force available to this Boid, just
	//add together. Otherwise add as much of the ForceToAdd vector is
	//possible without going over the max.
	if (MagnitudeToAdd < MagnitudeRemaining)
	{
		RunningTot += ForceToAdd;
	}

	else
	{
		//add it to the steering force
		RunningTot += ForceToAdd.normalisedCopy() * MagnitudeRemaining; 
	}

	return true;
}


//---------------------- CalculatePrioritized ----------------------------
//
//  this method calls each active steering behavior in order of priority
//  and accumulates their forces until the max steering force magnitude
//  is reached, at which time the function returns the steering force 
//  accumulated to that  point
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::CalculatePrioritized()
{
	MyVec3 force;

	if (On(wall_avoidance))
	{
		force = WallAvoidance(mBoid->getWorld()->Walls()->Walls()) *
			mdWeightWallAvoidance;

		if (!AccumulateForce(mSteeringForce, force)) return mSteeringForce;
	}

	if (On(obstacle_avoidance))
	{
		force = ObstacleAvoidance(mBoid->getWorld()->Obstacles()) * 
			mdWeightObstacleAvoidance;

		if (!AccumulateForce(mSteeringForce, force)) return mSteeringForce;
	}

	/*if (On(evade))
	{
		assert(mpTargetAgent1 && "Evade target not assigned");

		force = Evade(mpTargetAgent1) * mdWeightEvade;

		if (!AccumulateForce(mSteeringForce, force)) return mSteeringForce;
	}


	if (On(flee))
	{
		force = Flee(mBoid->getWorld()->Crosshair()) * mdWeightFlee;

		if (!AccumulateForce(mSteeringForce, force)) return mSteeringForce;
	}*/

	// TODO ..... add more stuff here

	if (On(follow_path))
	{
		force = FollowPath() * mdWeightFollowPath;

		if (!AccumulateForce(mSteeringForce, force)) return mSteeringForce;
	}

	return mSteeringForce;
}


//---------------------- CalculateWeightedSum ----------------------------
//
//  this simply sums up all the active behaviors X their weights and 
//  truncates the result to the max available steering force before 
//  returning
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::CalculateWeightedSum()
{
	if (On(wall_avoidance))
	{
		mSteeringForce += WallAvoidance(mBoid->getWorld()->Walls()->Walls()) *
			mdWeightWallAvoidance;
	}

	if (On(obstacle_avoidance))
	{
		mSteeringForce += ObstacleAvoidance(mBoid->getWorld()->Obstacles()) * 
			mdWeightObstacleAvoidance;
	}

	// ..... 
	if (On(wander))
	{
		mSteeringForce += Wander() * mdWeightWander;
	}

	// .....
	if (On(follow_path))
	{
		mSteeringForce += FollowPath() * mdWeightFollowPath;
	}

	mSteeringForce.truncate(mBoid->getMaxForce());

	return mSteeringForce;
}

//---------------------- CalculateDithered ----------------------------
//
//  this method sums up the active behaviors by assigning a probability
//  of being calculated to each behavior. It then tests the first priority
//  to see if it should be calculated this simulation-step. If so, it
//  calculates the steering force resulting from this behavior. If it is
//  more than zero it returns the force. If zero, or if the behavior is
//  skipped it continues onto the next priority, and so on.
//
//  NOTE: Not all of the behaviors have been implemented in this method
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::CalculateDithered()
{
	//reset the steering force
	mSteeringForce = MyVec3::ZERO;

	if (On(wall_avoidance) && MyCommon::Math::UnitRandom() < Prm.prWallAvoidance)
	{
		mSteeringForce = WallAvoidance(mBoid->getWorld()->Walls()->Walls()) *
			mdWeightWallAvoidance / Prm.prWallAvoidance;

		if (mSteeringForce!=MyVec3::ZERO)
		{
			mSteeringForce.truncate(mBoid->getMaxForce()); 

			return mSteeringForce;
		}
	}

	if (On(obstacle_avoidance) && MyCommon::Math::UnitRandom() < Prm.prObstacleAvoidance)
	{
		mSteeringForce += ObstacleAvoidance(mBoid->getWorld()->Obstacles()) * 
			mdWeightObstacleAvoidance / Prm.prObstacleAvoidance;

		if (mSteeringForce!=MyVec3::ZERO)
		{
			mSteeringForce.truncate(mBoid->getMaxForce()); 

			return mSteeringForce;
		}
	}

	return mSteeringForce;
}


//------------------------------- Seek -----------------------------------
//
//  Given a target, this behavior returns a steering force which will
//  direct the agent towards the target
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Seek(MyVec3 TargetPos)
{
	MyVec3 DesiredVelocity = (TargetPos - MyVec3(mBoid->getPos().ptr())).normalisedCopy()
		* mBoid->getMaxSpeed();

	return (DesiredVelocity - mBoid->getVelocity());
}

//----------------------------- Flee -------------------------------------
//
//  Does the opposite of Seek
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Flee(MyVec3 TargetPos)
{
	//only flee if the target is within 'panic distance'. Work in distance
	//squared space.
	MyVec3 DesiredVelocity = (MyVec3(mBoid->getPos().ptr()) - TargetPos).normalisedCopy() 
		* mBoid->getMaxSpeed();

	return (DesiredVelocity - mBoid->getVelocity());
}


//--------------------------- Arrive -------------------------------------
//
//  This behavior is similar to seek but it attempts to arrive at the
//  target with a zero velocity
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Arrive(MyVec3 TargetPos,
								 Deceleration deceleration)
{
	MyVec3 ToTarget = TargetPos - MyVec3(mBoid->getPos().ptr());

	//calculate the distance to the target
	MyReal dist = ToTarget.length();

	if (dist > 0)
	{
		//because Deceleration is enumerated as an int, this value is required
		//to provide fine tweaking of the deceleration..
		const MyReal DecelerationTweaker = 0.3;
		//calculate the speed required to reach the target given the desired
		//deceleration
		MyReal speed =  dist / ((MyReal)deceleration * DecelerationTweaker);     
		//make sure the velocity does not exceed the max
		speed = MyCommon::Math::Min(speed, mBoid->getMaxSpeed());
		//from here proceed just like Seek except we don't need to normalize 
		//the ToTarget vector because we have already gone to the trouble
		//of calculating its length: dist. 
		MyVec3 DesiredVelocity =  ToTarget * speed / dist;

		return (DesiredVelocity - mBoid->getVelocity());
	}

	return MyVec3::ZERO;
}

//------------------------------ Pursuit ---------------------------------
//
//  this behavior creates a force that steers the agent towards the 
//  evader
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Pursuit(const Boid* evader)
{
	//if the evader is ahead and facing the agent then we can just seek
	//for the evader's current position.
	MyVec3 ToEvader = MyVec3((evader->getPos() - mBoid->getPos()).ptr());

	MyReal RelativeHeading = mBoid->getHeading().dotProduct(evader->getHeading());

	if ( (ToEvader.dotProduct(mBoid->getHeading()) > 0) &&  
		(RelativeHeading < -0.95))  //acos(0.95)=18 degs
	{
		return Seek(MyVec3(evader->getPos().ptr()));
	}

	//Not considered ahead so we predict where the evader will be.

	//the lookahead time is proportional to the distance between the evader
	//and the pursuer; and is inversely proportional to the sum of the
	//agent's velocities
	MyReal LookAheadTime = ToEvader.length() / 
		(mBoid->getMaxSpeed() + evader->getSpeed());

	//now seek to the predicted future position of the evader
	return Seek(MyVec3(evader->getPos().ptr()) + evader->getVelocity() * LookAheadTime);
}

//----------------------------- Evade ------------------------------------
//
//  similar to pursuit except the agent Flees from the estimated future
//  position of the pursuer
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Evade(const Boid* pursuer)
{
	/* Not necessary to include the check for facing direction this time */

	MyVec3 ToPursuer = MyVec3((pursuer->getPos() - mBoid->getPos()).ptr());

	//uncomment the following two lines to have Evade only consider pursuers 
	//within a 'threat range'
	const MyReal ThreatRange = 100.0;
	if (ToPursuer.squaredLength() > ThreatRange * ThreatRange) return MyVec3();

	//the lookahead time is proportional to the distance between the pursuer
	//and the pursuer; and is inversely proportional to the sum of the
	//agents' velocities
	MyReal LookAheadTime = ToPursuer.length() / 
		(mBoid->getMaxSpeed() + pursuer->getSpeed());

	//now flee away from predicted future position of the pursuer
	return Flee(MyVec3(pursuer->getPos().ptr()) + pursuer->getVelocity() * LookAheadTime);
}

//--------------------------- Wander -------------------------------------
//
//  This behavior makes the agent wander about randomly
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::Wander()
{ 
	//this behavior is dependent on the update rate, so this line must
	//be included when using time independent framerate.
	MyReal JitterThisTimeSlice = mdWanderJitter * mBoid->getTimeElapsed();

	//first, add a small random vector to the target's position
	mvWanderTarget += MyVec3(
		MyCommon::Math::SymmetricRandom() * JitterThisTimeSlice,
		0,
		MyCommon::Math::SymmetricRandom() * JitterThisTimeSlice
		);

	//reproject this new vector back on to a unit circle
	mvWanderTarget.normalize();

	//increase the length of the vector to the same as the radius
	//of the wander circle
	mvWanderTarget *= mdWanderRadius;

	//move the target into a position WanderDist in front of the agent
	MyVec3 target = mvWanderTarget + MyVec3(mdWanderDistance,0,0);

	//project the target into world space
	/*MyVec3 Target = PointToWorldSpace(target,
		mBoid->getHeading(),
		mBoid->getSide(), 
		mBoid->getPos());*/

	//and steer towards it
	//return Target - mBoid->getPos(); 
	return MyVec3::ZERO;
}



//---------------------- ObstacleAvoidance -------------------------------
//
//  Given a vector of CObstacles, this method returns a steering force
//  that will prevent the agent colliding with the closest obstacle
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::ObstacleAvoidance(const EntityVector& obstacles)
{
	//the detection box length is proportional to the agent's velocity
	mdDBoxLength = Prm.MinDetectionBoxLength + 
		(mBoid->getSpeed()/mBoid->getMaxSpeed()) *
		Prm.MinDetectionBoxLength;

	//tag all obstacles within range of the box for processing
	//mBoid->getWorld()->TagObstaclesWithinViewRange(mVehicle, mdDBoxLength);

	//this will keep track of the closest intersecting obstacle (CIB)
	Ogre::Entity* ClosestIntersectingObstacle = NULL;

	//this will be used to track the distance to the CIB
	MyReal DistToClosestIP = MyCommon::Math::MAX_REAL;

	//this will record the transformed local coordinates of the CIB
	MyVec3 LocalPosOfClosestObstacle;

	EntityVector::const_iterator curOb = obstacles.begin();
	while (curOb!=obstacles.end())
	{
		//if the obstacle has been tagged within range proceed
		//if ((*curOb)->isTagged())
		if(true)
		{
			//calculate this obstacle's position in local space
			MyVec3 obPos((*curOb)->getParentSceneNode()->getPosition().ptr());
			MyVec3 localPos = PointToLocalSpace(obPos, mBoid->getHeading(),
				mBoid->getSide(),MyVec3(mBoid->getPos().ptr()));

			//if the local position has a negative x value then it must lay
			//behind the agent. (in which case it can be ignored)
			if (localPos.x >=0)
			{
				//if the distance from the x axis to the object's position is less
				//than its radius + half the width of the detection box then there
				//is a potential intersection.
				OgreReal ExpandedRadius = (*curOb)->getBoundingRadius() + mBoid->getEntity()->getBoundingRadius();

				if (fabs(localPos.y) < ExpandedRadius)
				{
					//now to do a line/circle intersection test. The center of the 
					//circle is represented by (cX, cY). The intersection points are 
					//given by the formula x = cX +/-sqrt(r^2-cY^2) for y=0. 
					//We only need to look at the smallest positive value of x because
					//that will be the closest point of intersection.
					MyReal cX = localPos.x;
					MyReal cY = localPos.y;

					//we only need to calculate the sqrt part of the above equation once
					double SqrtPart = sqrt(ExpandedRadius*ExpandedRadius - cY*cY);

					double ip = cX - SqrtPart;

					if (ip <= 0.0)
					{
						ip = cX + SqrtPart;
					}

					//test to see if this is the closest so far. If it is keep a
					//record of the obstacle and its local coordinates
					if (ip < DistToClosestIP)
					{
						DistToClosestIP = ip;
						ClosestIntersectingObstacle = *curOb;
						LocalPosOfClosestObstacle = localPos;
					}
				}
			}
		}
		++curOb;
	}

	//if we have found an intersecting obstacle, calculate a steering 
	//force away from it
	MyVec3 SteeringForce;

	if (ClosestIntersectingObstacle)
	{
		//the closer the agent is to an object, the stronger the 
		//steering force should be
		double multiplier = 1.0 + (mdDBoxLength - LocalPosOfClosestObstacle.x) /
			mdDBoxLength;

		//calculate the lateral force
		SteeringForce.y = (ClosestIntersectingObstacle->getBoundingRadius()-
			LocalPosOfClosestObstacle.y)  * multiplier; 

		//apply a braking force proportional to the obstacles distance from
		//the vehicle. 
		const double BrakingWeight = 0.2;

		SteeringForce.x = (ClosestIntersectingObstacle->getBoundingRadius() - 
			LocalPosOfClosestObstacle.x) * 
			BrakingWeight;
	}

	//finally, convert the steering vector from local to world space
	return VectorToWorldSpace(SteeringForce,
		mBoid->getHeading(),
		mBoid->getSide());
}

//--------------------------- WallAvoidance --------------------------------
//
//  This returns a steering force that will keep the agent away from any
//  walls it may encounter
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::WallAvoidance(const std::vector<Wall3D> &walls)
{
	//the feelers are contained in a std::vector, m_Feelers
	CreateFeelers();

	MyReal DistToThisIP    = 0.0;
	MyReal DistToClosestIP = MyCommon::Math::MAX_REAL;

	//this will hold an index into the vector of walls
	int ClosestWall = -1;

	MyVec3 SteeringForce,
		point,         //used for storing temporary info
		ClosestPoint;  //holds the closest intersection point

	//examine each feeler in turn
	for (unsigned int flr=0; flr<mFeelers.size(); ++flr)
	{
		//run through each wall checking for any intersection points
		for (unsigned int w=0; w<walls.size(); ++w)
		{
			MyCommon::Plane wallplane(walls[w].Normal(),walls[w].Center());
			std::pair<bool,MyReal> result = mFeelers[flr].intersects(wallplane);
			if (result.first)
			{
				//is this the closest found so far? If so keep a record
				if (DistToThisIP<DistToClosestIP)
				{
					DistToClosestIP=DistToThisIP;
					ClosestWall = w;
					ClosestPoint = mFeelers[flr].getPoint(result.second);
				}
			}
		}//next wall

		//if an intersection point has been detected, calculate a force  
		//that will direct the agent away
		if (ClosestWall >=0)
		{
			//calculate by what distance the projected position of the agent
			//will overshoot the wall
			MyVec3 OverShoot = mFeelers[flr].getDirection() - ClosestPoint;

			//create a force in the direction of the wall normal, with a 
			//magnitude of the overshoot
			SteeringForce = walls[ClosestWall].Normal() * OverShoot.length();
		}
	}//next feeler

	return SteeringForce;
}

//------------------------------- CreateFeelers --------------------------
//
//  Creates the antenna utilized by WallAvoidance
//------------------------------------------------------------------------
void SteeringBehavior::CreateFeelers()
{
	//feeler pointing straight in front
	mFeelers[0] = MyCommon::Ray(
		MyVec3(mBoid->getPos().ptr()), 
		mdWallDetectionFeelerLength*mBoid->getHeading());

	MyCommon::Quaternion quat;

	//feeler to left
	quat.FromAngleAxis(MyCommon::Radian(MyCommon::Math::HALF_PI*3.5),MyVec3::UNIT_Y);
	MyVec3 vdir = mdWallDetectionFeelerLength*0.5*(quat*mBoid->getHeading());
	mFeelers[1] = MyCommon::Ray(MyVec3(mBoid->getPos().ptr()),vdir);

	//feeler to right
	quat.FromAngleAxis(MyCommon::Radian(MyCommon::Math::HALF_PI*0.5),MyVec3::UNIT_Y);
	vdir = mdWallDetectionFeelerLength*0.5*(quat*mBoid->getHeading());
	mFeelers[2] = MyCommon::Ray(MyVec3(mBoid->getPos().ptr()),vdir);

}


//------------------------------- FollowPath -----------------------------
//
//  Given a series of Vector2Ds, this method produces a force that will
//  move the agent along the waypoints in order. The agent uses the
// 'Seek' behavior to move to the next waypoint - unless it is the last
//  waypoint, in which case it 'Arrives'
//------------------------------------------------------------------------
MyVec3 SteeringBehavior::FollowPath()
{
	//move to next target if close enough to current target (working in
	//distance squared space)
	if (mpPath->CurrentWaypoint().squaredDistance(MyVec3(mBoid->getPos().ptr())) < 
		mdWaypointSeekDistSq)
	{
		mpPath->SetNextWaypoint();
	}

	if (!mpPath->Finished())
	{
		return Seek(mpPath->CurrentWaypoint());
	}
	else
	{
		return Arrive(mpPath->CurrentWaypoint(),normal);
	}

}